#include "motor.h"
#include "main.h"
#include "gpio.h"
#include "tim.h"

void Motor_Init()
{
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_4);
  __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, 0);
  __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4, 0);
  
}

void Motor_SetPWM(int pwma, int pwmb)
{
  if( pwma > 7200 )   pwma = 7200;
  if( pwma <-7200 )   pwma =-7200;
  
  if( pwma > 0 )
  {
    HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
    __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4, pwma);
  }else if( pwma < 0 )
  {
    HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);
    __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4, -pwma);
  }else{
    HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
    __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4, 0);
  }
  
  if( pwmb > 7200 )   pwmb = 7200;
  if( pwmb <-7200 )   pwmb =-7200;
  if( pwmb > 0 )
  {
    HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
    __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, pwmb);
  }else if( pwmb < 0 )
  {
    HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_SET);
    __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, -pwmb);
  }else{
    HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
    __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, 0);
  }
}
